#include "fixed_lookuptbl.h"
#include "fixed64.h"
#include "coslooktbl.cpp"
#include "sinlooktbl.cpp"
#include "atanlooktbl.cpp"
#include <algorithm>

fixed64 fixedSin(const fixed64& radian)
{
	//range [-DPI,DPI]
	fixed64 rd = radian % fixed64::DPI;

	//range [-PI,PI]
	if (rd < -fixed64::PI)
		rd += fixed64::DPI;
	else if (rd > fixed64::PI)
		rd -= fixed64::DPI;

	//range [0,PI]
	FPRaw sign = 1;
	if (rd < fixed64::_0)
	{
		sign = -1;
		rd = -rd;
	}

	//range [0,PI/2)
	if (rd > fixed64::Rad_90)
	{
		rd = fixed64::PI - rd;
	}

	//to lookup range [0,1000)
	rd = (rd << 1) * FIXED_LOOKTABLE_UP_CNT / fixed64::PI;
	int idx = rd.floor();
	idx = idx < 0 ? 0 : (idx > (FIXED_LOOKTABLE_UP_CNT - 1) ? (FIXED_LOOKTABLE_UP_CNT - 1) : idx);
	FPRaw gv = g_table_sin[idx];
	rd = gv * sign;
	return rd;
}

fixed64 fixedCos(const fixed64& radian)
{
	//range [-DPI,DPI]
	fixed64 rd = radian % fixed64::DPI;

	//range [-PI,PI]
	if (rd < -fixed64::PI)
		rd += fixed64::DPI;
	else if (rd > fixed64::PI)
		rd -= fixed64::DPI;

	//range [0,PI]
	if (rd < fixed64::_0)
		rd = -rd;

	if (rd == fixed64::Rad_90)
		return fixed64::_0;

	//range [0,PI/2)
	FPRaw sign = 1;
	if (rd > fixed64::Rad_90)
	{
		sign = -1;
		rd = fixed64::PI - rd;
	}

	//to lookup range [0,1000)
	rd = (rd << 1) * FIXED_LOOKTABLE_UP_CNT / fixed64::PI;
	int idx = rd.floor();
	idx = idx < 0 ? 0 : (idx > (FIXED_LOOKTABLE_UP_CNT - 1) ? (FIXED_LOOKTABLE_UP_CNT - 1) : idx);

	rd = g_table_cos[idx] * sign;
	return rd;
}

fixed64 fixedTan(const fixed64& radian)
{
	fixed64 cosv = fixedCos(radian);
	fixed64 sinv = fixedSin(radian);
	if (cosv == fixed64::_0)
	{
		if (sinv > fixed64::_0)
			return fixed64::DRANGE_MAX;
		return fixed64::DRANGE_MIN;
	}
	return sinv / cosv;
}

fixed64 fixedATan2(const fixed64& y, const fixed64& x)
{
	if (y == fixed64::_0)
		return fixed64::_0;

	if (x == fixed64::_0)
	{
		if (y >= fixed64::_0)
			return fixed64::Rad_90;
		return -fixed64::Rad_90;
	}

	fixed64 tmax = fixed64(g_table_atan[FIXED_LOOKTABLE_UP_CNT - 1]);
	if (fixed64::abs(x) * tmax < fixed64::abs(y))
	{
		if ((x < fixed64::_0) == (y < fixed64::_0))
		{
			return fixed64::Rad_90;
		}
		return -fixed64::Rad_90;
	}

	FPRaw value = (fixed64::abs(y) / fixed64::abs(x));
	int low = 0;
	int high = FIXED_LOOKTABLE_UP_CNT - 1;
	int mid = 0;
	FPRaw midValue = 0;
	while (low < high)
	{
		mid = (low + high) >> 1;
		midValue = g_table_atan[mid];
		if (mid == low || midValue == value)
		{
			if ((x < fixed64::_0) == (y < fixed64::_0))
				return fixed64::Rad_90 * mid >> FIXED_LLOKTABLE_BIT;
			return -(fixed64::Rad_90 * mid >> FIXED_LLOKTABLE_BIT);
		}

		if (midValue < value)
			low = mid;
		else
			high = mid;
	}

	if ((x < fixed64::_0) == (y < fixed64::_0))
		return fixed64::Rad_90 * mid >> FIXED_LLOKTABLE_BIT;
	return -(fixed64::Rad_90 * mid >> FIXED_LLOKTABLE_BIT);
}